function [ww,auxi_vari,num_loop] = alg_Direct_FP(N,K,MG,hGG,noise,pow_max)
ww = initialize_w(N,K,MG,hGG,pow_max);
fq_record = [];
f_quadratic_old = 1e-10;
sum_rate_terres_record = [];
sum_rate_terres_old = 1e-10;
num_loop=0;
while true
    %% update U V W
    auxi_vari = update_Direct_FP_auxiliary_variables(N,K,ww,hGG,noise);
    [ww,cvx_status,f_quadratic] = pro_Direct_FP(N,K,MG,ww,hGG,pow_max,auxi_vari,noise);
    if ~strcmp(cvx_status,'Solved') && ~strcmp(cvx_status,'Inaccurate/Solved')
        fprintf(['cvx status: ',cvx_status,' num_loop=',num2str(num_loop),'\n'])
        hhhhhhhhhhhh
    end
    num_loop=num_loop+1;

    %% converge?????????????
    fq_record = [fq_record;f_quadratic];   
%     if abs(f_quadratic-f_quadratic_old) / f_quadratic_old < 0.01
%         fprintf('Direct FP terminates: f_quadratic converges\n');
%         break
%     else
%         f_quadratic_old = f_quadratic;
%     end
    
    %% converge?????????????
    sum_rate_terres = cal_sum_rate_terres(N,K,ww,hGG,noise);
    sum_rate_terres_record = [sum_rate_terres_record;sum_rate_terres]; % 用于绘图
    if abs(sum_rate_terres-sum_rate_terres_old) / sum_rate_terres_old < 0.001
        fprintf('Direct FP terminates: sum_rate_terres converges\n');
        break
    else
        sum_rate_terres_old = sum_rate_terres;
    end
end

figure
subplot(2,1,1)
plot(1:length(fq_record),fq_record,'o-');hold on
ylabel('Quadratic objective')
% New objective by applying quadratic transform
grid on
% xlabel('The number of iterations of Direct FP')
set(gca, 'FontName', 'Times New Roman','FontSize',12)
subplot(2,1,2)
plot(1:length(sum_rate_terres_record),sum_rate_terres_record,'x-');hold on
ylabel('Sum rate (bps/Hz)')
% ylabel('sum_rate_terres')
grid on
xlabel('The number of iterations of Direct FP')
set(gca, 'FontName', 'Times New Roman','FontSize',12)
% set(ff,'Position',[1200,300,500,400]);
end

function auxi_vari = update_Direct_FP_auxiliary_variables(N,K,ww,hGG,noise)
auxi_vari=zeros(N,K);% real !!!!!!!!!
for ii=1:N
    for mm=1:K
        sum_wmlH_hmnk = 0;
        for jj=1:N
            for nn=1:K
                if jj~=ii || nn~=mm
                    sum_wmlH_hmnk = sum_wmlH_hmnk + hGG{jj,ii,mm}'*ww(:,jj,nn)*ww(:,jj,nn)'*hGG{jj,ii,mm};
                end
            end
        end
        auxi_vari(ii,mm) = hGG{ii,ii,mm}'*ww(:,ii,mm) / (noise + real(sum_wmlH_hmnk));
    end
end
end

function [ww,cvx_status,cvx_optval] = pro_Direct_FP(N,K,MG,ww,hGG,pow_max,auxi_vari,noise)
cvx_begin sdp quiet
variable ww(MG,N,K) complex
expression cons_sum_power(N,1)
maximize( compute_fq_Direct(N,K,auxi_vari,ww,hGG,noise))
subject to
cal_sum_power_Direct_FP(N,K,ww,cons_sum_power) <= pow_max;
cvx_end
end

function f_quadratic = compute_fq_Direct(N,K,auxi_vari,ww,hGG,noise)
f_quadratic=0;
% scale_factor = 1e6;
for ii=1:N
    for mm=1:K
        sum_wmlH_hmnk = 0;
        for jj=1:N
            for nn=1:K
                if jj~=ii || nn~=mm
                    sum_wmlH_hmnk = sum_wmlH_hmnk + ww(:,jj,nn)'*(hGG{jj,ii,mm}*hGG{jj,ii,mm}')*ww(:,jj,nn);
                end
            end
        end
        temp1 = 1 + 2 * real(auxi_vari(ii,mm)' * hGG{ii,ii,mm}'*ww(:,ii,mm));
        temp2 = real(auxi_vari(ii,mm)' * auxi_vari(ii,mm)) * (noise + real(sum_wmlH_hmnk));
%         f_quadratic = f_quadratic + log((temp1 - temp2)*scale_factor) - log(scale_factor);
        f_quadratic = f_quadratic + log(temp1 - temp2);
    end
end
end

function cons_sum_power = cal_sum_power_Direct_FP(N,K,ww,cons_sum_power)
% cons_sum_power = zeros(N,1);
for nn=1:N
    temp1 = 0;
    for kk=1:K
        temp1 = temp1 + (ww(:,nn,kk)'*ww(:,nn,kk));
    end
    cons_sum_power(nn) = temp1;
end
cons_sum_power = real(cons_sum_power);
end

